﻿#include "../header/TSP_solver.hpp"

#include <fstream>


double randn(double mu, double sigma)
{
    double U1, U2, W, mult;
    static double X1, X2;
    static int call = 0;
 
    if (call == 1)
    {
        call = !call;
        return (mu + sigma * (double) X2);
    }
 
    do
    {
        U1 = -1 + ((double) rand () / RAND_MAX) * 2;
        U2 = -1 + ((double) rand () / RAND_MAX) * 2;
        W = pow (U1, 2) + pow (U2, 2);
    }
    while (W >= 1 || W == 0);
 
    mult = sqrt ((-2 * log (W)) / W);
    X1 = U1 * mult;
    X2 = U2 * mult;
 
    call = !call;
 
    return (mu + sigma * (double) X1);
}


TSP_solver::TSP_solver(const char* datasetFile) {
    cout<<"www"<<endl;
    int op = 0;
    inputData(datasetFile);
    cin>>op;
    switch(op){
        case 0:
            solveWithLocalsearch(30);
            break;
        case 1:
            solveWithFile();
            break;
        default:
            solveWithGenaration();
            break;
    }
    showSolving(10);
}

void TSP_solver::inputData(const char* datasetFile) {
    fstream gFile;
    cout<<datasetFile<<endl;
    gFile.open(datasetFile, ios::in );
    //获取文件大小
    gFile.seekg(0, ios_base::end);
    int nFileLen = gFile.tellg();
    
    gFile.seekg(0);
    char buffer[20]={0};
    int pointSeq=0,x=0,y=0;
    // cout<<nFileLen<<endl;
    // get a line
    max_x=0;
    max_y=0;
    gFile>>pointSeq>>x>>y;
    while(pointSeq>0) {
        Point point(x, y);
        pointMap[pointMap.size()]=point;
        cout<<x<<":"<<y<<endl;
        // cout<<pointSeq<<" : "<<x<<" : "<<y<<endl;
        max_x=max_x>=x?max_x:x;
        max_y=max_y>=y?max_y:y;
        gFile>>pointSeq>>x>>y;
    }
    gFile.close();
}

void TSP_solver::solveWithGreedy(){
    vector<int> unselect;
    for(int c1=1;c1<pointMap.size();c1++){
        unselect.push_back(c1);
    }

    solve.clear();
    solve.push_back(0);
    for(int c1=1;c1<pointMap.size();c1++){
        
        float miniCost = dist(pointMap[solve[c1-1]],pointMap[unselect[0]]);
        int miniIndex = 0;
        float tempCost=0;
        for(int c2=1;c2<unselect.size();c2++){
            tempCost = dist(pointMap[solve[c1-1]],pointMap[unselect[c2]]);
            if(tempCost<miniCost){
                miniCost = tempCost;
                miniIndex = c2;
            }
        }

        solve.push_back(unselect[miniIndex]);
        unselect.erase(unselect.begin()+miniIndex);
    }
}

void TSP_solver::solveWithLocalsearch(int radius){
    // random start,no,just input rank
    srand(time(0));

    vector<int> unselect;
    int point_count = pointMap.size();

    srand(time(0));

    for(int c1=0;c1<pointMap.size();c1++){
        unselect.push_back(c1);
    }
    
    while(unselect.size()!=0){
        int tempIndex = rand()%unselect.size();
        solve.push_back(unselect[tempIndex]);
        unselect.erase(unselect.begin()+tempIndex);
    }

    int current_index = rand()%(pointMap.size()-1);

    int cost = getTotalCost(solve),tempCost=0;
    int rounds = 0,over=0, mini=0;
    while(1){
        vector<int> temp = solve;
        variation(temp);

        tempCost = getTotalCost(temp);
        if(tempCost<cost){
            solve = temp;
            cost = tempCost;
        }

        // next
        current_index = (current_index+1)%solve.size();
        rounds++;
        if(rounds%6000==0){
            cout<<"ROUND"<<rounds<<":"<<cost<<endl;
            showSolving(rounds);
        }

        if(over>10000){
            cout<<"ROUND"<<rounds<<":"<<cost<<endl;
            break;
        }
        
        if(mini == cost){
            over++;
        } else {
            mini = cost;
            over = 0;
        }
    }

    cout<<"-----final: "<<":"<<cost<<"-----"<<endl;
    showSolving(rounds);
    showSolve();
}


void TSP_solver::solveWithFile(){
    // random start,no,just input rank
    vector<int> unselect;
    int point_count = pointMap.size();

    srand(time(0));

    for(int c1=0;c1<pointMap.size();c1++){
        unselect.push_back(c1);
    }
    
    while(unselect.size()!=0){
        int tempIndex = rand()%unselect.size();
        solve.push_back(unselect[tempIndex]);
        unselect.erase(unselect.begin()+tempIndex);
    }

    // solveWithGreedy();

    // cout<<solve.size()<<endl;
    int current_index = rand()%pointMap.size();

    int cost = getTotalCost(solve),tempCost=0;
    int rounds = 0,over=0;
    //T=1000000 L=0.8 C=3 R=20
    double T=1000000;//atof(T_s);
    double L=1;//atof(L_s);
    int Total_C=3;//atoi(C_s);
    double R=5;//atof(R_s);
    cout<<"please input T,L,C,R"<<endl;
    cin>>T>>L>>Total_C>>R;
    
    int C=Total_C-1; //atoi(C_s)-1;
    int radius = 3;
    int bestCost=getTotalCost(solve);
    vector<int> bestSolve = solve;
    int mini=bestCost;
    vector<int> miniSolve = solve;

    vector<int> point_map_selected;
    for(int c1=0;c1<5;c1++){
        point_map_selected.push_back(c1);
    }
    vector<int> collect;

    while(1){
        for(int ct = 0; ct<1000;ct++){
            vector<int> temp = solve;
            variation(temp);

            tempCost = getTotalCost(temp);
            if(tempCost<cost || (T!=0 &&(((double)rand())/RAND_MAX <  1.0/(1+exp((tempCost-cost)/T))))){
                solve = temp;
                cost = tempCost;
            }

            // next
            current_index = rand()%pointMap.size();
            rounds++;

            if(mini > cost){
                mini = cost;
                miniSolve = solve;
                over = 0;
            } else {
                if(T<=L)
                    over++;
            }

            if(rounds%5000==0){
                showSolving(rounds);
                cout<<"ROUND"<<rounds<<":"<<cost<<endl;
            }

            if(over>300000){
                srand(time(0));
                // reset to the best
                if(bestCost > mini){
                    bestCost = mini;
                    bestSolve = miniSolve;
                    cout<<"------GOOD ROUNDS:"<<C<<"---CURRENT cost:"<<bestCost<<endl;
                } else {
                    mini = bestCost;
                    miniSolve = bestSolve;
                    cout<<"------BAD ROUNDS:"<<C<<"---CURRENT cost:"<<bestCost<<endl;
                }
                C--;
                over = 0;
                solve = miniSolve;
                T=R;

                if(C<=0)
                {
                    cout<<"ROUND"<<rounds<<":"<<cost<<endl;
                    solve= bestSolve;
                    cout<<"------FINAL:"<<C<<"---cost:"<<getTotalCost(solve)<<endl;
                    showSolving(rounds);
                    showSolve();
                    exit(0);
                }
            }
        }
        if(T>L)
            T=0.99*T;

    }
    
}

void TSP_solver::solveWithGenaration(){
    cout<<"into solver"<<endl;
    // char *G_s = getenv("G");

    int Genaration=10000;
    int gcount=0;
    srand(time(0));
    int totalSize = 500;
    // 初始化为160个个体,随机顺序
    int cityCount = pointMap.size();
    vector<solution> solutionsPool;

    cout<<"please input crossRate%100,variationRate%1000:"<<endl;

    // 交叉概率,百分比 60-100
    int crossRate=95;
    // 变异概率 // 5-10
    int variationRate = 10;
    cin>>crossRate>>variationRate;
    // this->solveWithLocalsearch(30);

    // solveWithGreedy();
    solution initso;
    // initso.solve = solve;
    // initso.cost = getTotalCost(initso.solve);

    // solutionsPool.push_back(initso);
    // solutionsPool.push_back(initso);

    initso.solve.clear();

    for(int c1=0;c1<cityCount;c1++){
        initso.solve.push_back(c1);
    }
    initso.cost = getTotalCost(initso.solve);


    solutionsPool.push_back(initso);
    // solutionsPool.push_back(initso);
    cout<<"init pool"<<endl;
    for(int c1=0;c1<totalSize;c1++){
        vector<int> unselect;
        solution initSolution;
        for(int c1=0;c1<pointMap.size();c1++){
            unselect.push_back(c1);
        }
        while(unselect.size()!=0){
            int tempIndex = rand()%unselect.size();
            initSolution.solve.push_back(unselect[tempIndex]);
            unselect.erase(unselect.begin()+tempIndex);
        }
        initSolution.cost = getTotalCost(initSolution.solve);
        solutionsPool.push_back(initSolution);
    }
    solutionsPool.push_back(initso);

    cout<<"begin to generate"<<endl;
    while(1){
        vector<solution> nextGenarationPool;
        vector<solution> parentGenarationPool = solutionsPool;
        int currentSolveCount = solutionsPool.size();
        // cout<<"begin to copy"<<endl;
        // 繁衍,保留父辈，不需要自行添加


        // cout<<"begin to change"<<endl;
        // 池里面的解，重组&变异
        while(solutionsPool.size()>1){        
            // random two entities
            int solveLeft = rand()%solutionsPool.size();
            solution left= solutionsPool[solveLeft];
            solutionsPool.erase(solutionsPool.begin()+solveLeft);
            
            int solveRight = rand()%solutionsPool.size();
            solution right = solutionsPool[solveRight];
            solutionsPool.erase(solutionsPool.begin()+solveRight);

            // cout<<"begin to cross"<<endl;
            // 是否交叉
            if(rand()%100 <crossRate){
                // do cross
                // random size to swap
                int startPoint = rand()%cityCount;
                // int endPoint = (rand()%(cityCount-2)+1+startPoint)%cityCount;
                // int endPoint = (5+startPoint)%cityCount;
                int endPoint = (rand()%(cityCount/10)+1+startPoint)%cityCount;
                // int swapSize = startPoint<endPoint?endPoint - startPoint:startPoint - endPoint;

                for(int c1=startPoint;c1!=endPoint;c1=(c1+1)%cityCount){
                    int temp = left.solve[c1];
                    left.solve[c1] = right.solve[c1];
                    right.solve[c1] = temp;
                }

                solveConflictForSolution(left,right,startPoint,endPoint,cityCount);
                solveConflictForSolution(right,left,startPoint,endPoint,cityCount);
            }   

            // cout<<"begin to variation"<<endl;
            // 变异
            if(rand()%1000 > variationRate)
                variation(left.solve);
            if(rand()%1000 > variationRate)
                variation(right.solve);

            // update cost
            left.cost = getTotalCost(left.solve);
            right.cost = getTotalCost(right.solve);

            nextGenarationPool.push_back(left);
            nextGenarationPool.push_back(right);
        }

        solutionsPool.clear();

        for(int c1=0;c1<currentSolveCount;c1++){
            nextGenarationPool.push_back(parentGenarationPool[c1]);
        }

        parentGenarationPool.clear();

        // 排位概率的方法，高的高概率留下
        sort(nextGenarationPool.begin(),nextGenarationPool.end(),[](solution& a, solution &b){
            return a.cost<b.cost;
        });

        
        int bestCost = nextGenarationPool[0].cost;
        
        // 最好的个体留下的概率
        float q = 0.98;
        // float a = q/(2*2*2*totalSize*totalSize) * 1000;
        for(int c1=0;c1<nextGenarationPool.size();c1++){
            // p =1000*p 
            float p = 1000;
            // 非线性糟糕透了
            // for(int c2=0;c2<c1;c2++){
            //     p*=(1-q);
            // }
            // 线性也并没有多好，但还过得去
            p *= q - c1*q/totalSize/2;

            if(rand()%1000 <p){
                solutionsPool.push_back(nextGenarationPool[c1]);
            }
        }
        nextGenarationPool.clear();
        if(gcount%200==0){
            this->solve = solutionsPool[0].solve;
            showSolving(gcount);
            cout<<"FINISHED:"<<gcount*1.0/Genaration<<endl<<"temp mini cost:"<<bestCost<<endl
                <<"currrent size:"<<solutionsPool.size()<<endl;
        }

        if(++gcount%Genaration == 0){
            cout<<"finish?y/n"<<endl;
            char op = 0;
            cin>>op;
            if(op == 'y'||op == 'Y'){
                break;
            }
        }
    }

    // get the best one
    solution bestSolution = solutionsPool[0];
    for(int c1=1;c1<solutionsPool.size();c1++){
        if(solutionsPool[c1].cost<bestSolution.cost){
            bestSolution = solutionsPool[c1];
        }
    }

    cout<<"finish to generate"<<endl;
    solve = bestSolution.solve;
}

int TSP_solver::solveConflictForSolution(solution& left,solution& right, int startPoint,int endPoint,int cityCount){
    int tempLeftMid=startPoint;
    // fix error
    for(int c1=endPoint;c1!=startPoint;c1=(c1+1)%cityCount){
        // for left solution
        for(int c2=startPoint;c2!=endPoint;c2=(c2+1)%cityCount){
            if(left.solve[c1]==left.solve[c2]){
                // get a conflic,give a point to it
                // search the orignal to give it one,which donot in the new one
                for(int c3=tempLeftMid;c3!=endPoint;c3= (c3+1)%cityCount){
                    int c4=0;
                    for(c4=startPoint;c4!=endPoint;c4=(c4+1)%cityCount){
                        if(left.solve[c4] == right.solve[c3]){
                            break;
                        }
                    }

                    if(c4==endPoint){
                        left.solve[c1] = right.solve[c3];
                        tempLeftMid=(c3+1)%cityCount;
                        c2 = endPoint-1;
                        break;
                    }
                }
            }
        }
    }
}

// 变异
void TSP_solver::variation(vector<int>& currentsolve){
    if(currentsolve.size()!=pointMap.size()){
        cout<<"ERROR IN SIZE!"<<endl;
        exit(1);
    }


    int op =(int)(rand()%4);
    
    int current_index = rand()%currentsolve.size();
    int offset=1; 
    int tempPoint=0;

    offset= abs(randn(0,currentsolve.size()/6))+1;
    if(offset > currentsolve.size()-1)
        offset = 1;
    if(offset<=1){
        offset=2;   
    }
    switch (op)
    {
        case 0:

            for(int c1=0;c1<offset/2;c1++){
                tempPoint  = currentsolve[(current_index+c1)%currentsolve.size()];    
                currentsolve[(current_index+c1)%currentsolve.size()] = currentsolve[(current_index+offset -c1)%currentsolve.size()];
                currentsolve[(current_index+offset -c1)%currentsolve.size()] = tempPoint;
            }
            break;
        case 1:
            tempPoint = currentsolve[current_index]; 
            for(int c1=0;c1<offset;c1++){
                currentsolve[(current_index+c1)%currentsolve.size()] = currentsolve[(current_index+c1+1)%currentsolve.size()];
            }
            currentsolve[(current_index+offset)%currentsolve.size()] = tempPoint;
            break;
        default:
            tempPoint = currentsolve[(current_index)%currentsolve.size()]; 
            currentsolve[(current_index)%currentsolve.size()] = currentsolve[(current_index+offset)%currentsolve.size()];
            currentsolve[(current_index+offset)%currentsolve.size()] = tempPoint;
            break;
    }
}

int TSP_solver::getTotalCost(vector<int> solve){
    float res=0;
    for(int current_index=0;current_index<solve.size();current_index++){
        res+=dist(pointMap[solve[current_index]],pointMap[solve[(current_index+1)%pointMap.size()]]);
    }

    return res;
}

void TSP_solver::showPoint() {
    int count = 0;
    CImg<unsigned char> lt_map(max_x*margin+2*padding,max_y*margin+2*padding,1,3,0);
    for(auto &v : solve){
        count++;
        Point point=pointMap[v];
        lt_map.draw_point(point.x*margin+padding,(max_y-point.y)*margin+padding,red,1);
    }
    cout<<"count: "<<count<<"  :  "<<pointMap.size()<<endl;

    lt_map.display("point map");
}

void TSP_solver::showSolve() {
    
    int maxY = 0;
    int minY = costChange[0];

    for(auto &item: costChange)
    {
        if(item>maxY){
            maxY = item;
        }

        if(item<minY){
            minY = item;
        }
    }

    CImg<unsigned char> lt_map(maxWidth*margin+2*padding,maxHeight*margin+2*padding,1,3,0);

    for(int c1=0;c1<costChange.size()-1;c1++){
        lt_map.draw_line((int)(c1*1.0f/costChange.size()*maxWidth*margin+padding),(int)((maxHeight -(costChange[c1]-minY)/1.0f/(maxY-minY)*maxHeight)*margin+padding),
            (int)((c1+1)*1.0f/costChange.size()*maxWidth*margin+padding),(int)((maxHeight -(costChange[c1+1]-minY)/1.0f/(maxY-minY)*maxHeight)*margin+padding),red);
        // printf("%d : %d  %d : %d\n",(int)(c1*1.0f/costChange.size()*maxWidth*margin+padding),(int)((maxHeight -(costChange[c1]-minY)/1.0f/(maxY-minY)*maxHeight)*margin+padding),
        //     (int)((c1+1)*1.0f/costChange.size()*maxWidth*margin+padding),(int)((maxHeight -(costChange[c1+1]-minY)/1.0f/(maxY-minY)*maxHeight)*margin+padding));
    }

    
    lt_map.display("change.bmp");
}

void TSP_solver::showSolving(int seq) {

    costChange.push_back(getTotalCost(solve));

    CImg<unsigned char> lt_map(max_x*margin+2*padding,max_y*margin+2*padding,1,3,0);
    for(auto &v : solve){
        Point point=pointMap[v];
        lt_map.draw_circle(point.x*margin+padding,(max_y-point.y)*margin+padding,1,blue);
    }

    for(int c1=0;c1<solve.size();c1++){
        Point point1=pointMap[solve[c1]];
        Point point2=pointMap[solve[(c1+1)%solve.size()]];
        lt_map.draw_line(point1.x*margin+padding,(max_y-point1.y)*margin+padding,
            point2.x*margin+padding,(max_y-point2.y)*margin+padding,red);
    }

    string num = to_string(seq);

    while(num.size()!= 10){
        num.insert(num.begin(),'0');
    }

    lt_map.save(("../img/res" + num+".bmp").c_str());
}

